/**
 * This class uses path normalised squared angle smoothing algorithm
 * 
 */

package tools;

import java.util.Arrays;

import neuralnetwork.SmoothingAlgorithm;

import org.apache.commons.math3.analysis.polynomials.PolynomialFunction;
import org.apache.commons.math3.geometry.euclidean.twod.Line;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
import org.apache.commons.math3.util.FastMath;
import org.apache.log4j.Level;
import org.apache.log4j.Logger;
import org.apache.log4j.PropertyConfigurator;

import interfaces.IGetSmoothing;
import interfaces.ILinearEquation;

/**
 * @author Vivek
 * 
 */
public class CalculatePathLengthNormalizedSmoothing implements IGetSmoothing {

	private PolynomialFunction univariate;
	private double[] angles;
	private double[] segLength;
	
//	static final Logger logger = Logger.getLogger(CalculatePathLengthNormalizedSmoothing.class); // Setup logging

	
	public CalculatePathLengthNormalizedSmoothing() {

        // loading logging properties
//        PropertyConfigurator.configure("Logging Properties/log4j.properties");

	}


	@Override
	public double getSmoothnessValue(double[][] xy) {

//		logger.setLevel(Level.FATAL);
		
		
		ILinearEquation lEq = new PolynomialGenerator(xy);
		
		lEq.solveEquation();
		lEq.getPredicted(2);
//		logger.info(UtilFunctions.display2DArray(xy));
//		((LinearEquationToolkit_v1) lEq).displayPoly();
//		System.out.println(lEq.getEquation());			
		
		double[][] xy_predicted = lEq.getPredicted(2);
		
		
//		logger.info("Predicted values"+UtilFunctions.display2DArray(xy_predicted));
		
		
		PolynomialFunction univariate= new PolynomialFunction(((PolynomialGenerator) lEq).solution.toArray());
		
//		System.out.println("Poly display :"+univariate.toString());
		
		double arcLength = getPolynomialArcLength(univariate,
				xy_predicted[0][0], xy_predicted[xy_predicted.length-1][0], 200);
		
//		System.out.println("Arc Length :"+arcLength);
		setup(xy_predicted);		
		
		return sumAnglesSegmentLength()*arcLength;
		
		
	}

	
	/**
	 * @param xy
	 */
	public void setup(double[][] xy) {
		Vector2D[] points = new Vector2D[xy.length];
		Line[] lines = new Line[xy.length - 1];
		this.segLength = new double[xy.length - 1];

		
//		System.out.println("Length :"+xy.length);
				
		for (int n = 0; n < xy.length; n++) {
			points[n] = new Vector2D(xy[n][0], xy[n][1]);
			
//			System.out.println(lines[n].toString()+ "111 n:" + n + " Len:" + xy.length);
			/*
			 * This code replaces the for loop below... saves a loop....
			 */
			if (n >= 1) {
				lines[n - 1] = new Line(points[n - 1], points[n]);
				System.out.println(lines[n-1]+ " X=" + points[n - 1].getX() + " Y=" + points[n - 1].getY());
//				System.out.println("Angle :"+Maths.lines[n - 1].getAngle());
				segLength[n - 1] = Vector2D.distance(points[n - 1], points[n]);
			
			}
		}

		/*
		 * Creates the lines and calculates the length of each line
		 */
		// for(int n=1; n < xy.length;n++){
		// System.out.println("n:"+n+" Len:"+xy.length);
		// lines[n-1] = new Line(points[n-1],points[n]);
		// length[n-1]=Vector2D.distance(points[n-1],points[n]);
		// }

		getLineAngle(lines);

	}

	/**
	 * This function calculates angles between lines
	 * 
	 * @param lines
	 */
	public void getLineAngle(Line[] lines) {
		angles = new double[lines.length-1];
		for (int n = 1; n < lines.length; n++) {
//			System.out.println("n:" + n);
			System.out.println("n:" + n + " " + lines[n].getAngle());
			angles[n - 1] = (lines[n].getAngle() - (lines[n - 1].getAngle()));
		}

	}
	

	/**
	 * This function takes sum of all the angles and segment length
	 * 
	 */
	public double sumAnglesSegmentLength() {
		double totalAngleLength = 0;
		
		for (int n = 1; n < segLength.length; n++) {
			totalAngleLength = totalAngleLength + segLength[n]*angles[n-1];
		}
		
		return totalAngleLength;
	}

	
	

	/**
	 * This function is used to calculate calculate the arc length of an
	 * polynomial. Specifically this function creates an array of points on
	 * curve at regular intervals (that is passed as parameters)
	 * 
	 * Its makes a call to getPolynomialLength to calculate the sum length for
	 * all the points.
	 * 
	 * @param univariate
	 *            Polynomial function whose arc length needs to be calculates
	 * @param start
	 *            X coordinate for the start point.
	 * @param end
	 *            X coordinates for end point.
	 * @param interval
	 *            total number of interval to be considered for calculation of
	 *            arc length
	 * @return
	 */
	private double getPolynomialArcLength(PolynomialFunction univariate,
			double start, double end, int interval) {
		double step = (double) (end - start) / interval;
		double[][] coord = new double[1 + interval][2];

		for (int counter = 0; counter <= interval; counter++) {
			coord[counter][0] = start + counter * step;
			coord[counter][1] = univariate.value(coord[counter][0]);

		}
		// System.out.println(Arrays.toString(coord[0]));
		// System.out.println(Arrays.toString(coord[1]));
		// System.out.println("Lenght"+coord.length+" Step:"+step);
		return getPolynomialLength(coord);
	}

	public static double getPolynomialLength(double[][] xy) {
		double length[] = new double[xy.length - 1];

		for (int n = 1; n < xy.length; n++) {
			length[n - 1] = Math.sqrt(Math.pow((xy[n - 1][0] - xy[n][0]), 2)
					+ Math.pow((xy[n - 1][1] - xy[n][1]), 2));
			// System.out.println("HEre :"+length[n-1]);
		}
		double sum = 0;
		for (double i : length) {
			sum += i;
		}

		return sum;
	}

	@Override
	public String getAlgorithmName() {
		return "Path length normalised Squared angle method";
	}

}
